#include "pid.h"

/**
 * 函数：pid_init
 * 功能：初始化PID各参数
 * 参数：pid:pid的结构体地址
 *       p: pid的静态kp值
 *       i: pid的静态ki值
 *       d: pid的静态kd值
 *      maxI:pid计算后的I最大值，即最大误差累加值
 *      maxOut:pid最大输出值
 *      target: 目标值
 */
void pid_init(PID* pid, float p, float i, float d, float maxI, float maxOut, int target)
{
    pid->kp = p;
    pid->ki = i;
    pid->kd = d;
    pid->max_change_i = maxI;
    pid->max_output = maxOut;
    pid->target = target;
}

/**
 * 函数：pid_change_zero
 * 功能：PID变化累计参数清零
 * 参数：PID对应的PID地址
 */
void pid_change_zero(PID *pid)
{
    pid->change_p = 0;
    pid->change_i = 0;
    pid->change_d = 0;
}

/**
 * 函数：pid_calc
 * 功能： 单级pid计算
 * 参数： pid:pid的参数输入
 *      target : 目标值
 *      current : 当前值
 * 返回： pid的计算结果
 */
float pid_calc(PID *pid, float target, float current)
{
    //用上一次误差值更新之前误差last_error
    pid->last_error = pid->error;
    //获取新的误差 = 目标值 - 当前值
    pid->error = target - current;

    //计算比例p = 目标值与实际值之间的误差e
    float pout = pid->error;
    //计算几分I = 误差e的累加
    pid->change_i += pid->error;
    //计算微分d = 当前误差 - 之前的误差last_e
    float dout = pid->error - pid->last_error;

    //积分I 限制不能超过正负最大值
    if(pid->change_i > pid->max_change_i)
    {
        pid->change_i = pid->max_change_i;
    }
    else if(pid->change_i < -pid->max_change_i)
    {
        pid->change_i = -pid->max_change_i;
    }

    //计算pid_out = (kp x p) + (ki x i) + (kd x d)
    pid->output = (pid->kp * pout) + (pid->ki * pid->change_i) + (pid->kd * dout);

    //输出限制不能超过正负最大值
    if(pid->output > pid->max_output)
        pid->output = pid->max_output;
    else if(pid->output < -pid->max_output)
        pid->output = -pid->max_output;

    return pid->output;
}

/**
 * 函数：pid_cascade_calc
 * 功能：串级PID计算
 * 参数：pid：串级PID结构体
 * outerRef：外环目标值
 * outerFdb：外环反馈值
 * innerFdb：内环反馈值
 */
void pid_cascade_calc(CascadePID *pid, float outerRef, float outerFdb, int innerFdb)
{
    pid_calc(&pid->outer, outerRef, outerFdb);  //计算外环
    pid_calc(&pid->inner, pid->outer.output, innerFdb);     //计算内环
    //内环输出就是串级PID的输出
    pid->output = pid->inner.output;
}

